use core::{marker::PhantomData, mem::MaybeUninit, ptr::null_mut};

use utils::MutCell;

use crate::{
    c_src::{
        osMessageQueueAttr_t, osMessageQueueGet, osMessageQueueId_t, osMessageQueueNew,
        osMessageQueuePut, osMessageQueueReset, osRtxMessageQueue_t, osWaitForever,
    },
    osStatus_t,
};

use super::{Error, StaticMem};

const CB_SIZE: usize = size_of::<osRtxMessageQueue_t>();

const fn mq_size_impl(msg_count: usize, msg_size: usize) -> usize {
    4 * (msg_count) * (3 + (((msg_size) + 3) / 4))
}

pub const fn mq_size<T>(msg_count: usize) -> usize {
    mq_size_impl(msg_count, size_of::<T>())
}

unsafe impl<T, const MSG_COUNT: usize, const MQ_SIZE: usize> Send
    for MessageQueue<T, MSG_COUNT, MQ_SIZE>
{
}
unsafe impl<T, const MSG_COUNT: usize, const MQ_SIZE: usize> Sync
    for MessageQueue<T, MSG_COUNT, MQ_SIZE>
{
}

pub struct MessageQueue<T, const MSG_COUNT: usize, const MQ_SIZE: usize> {
    _p: PhantomData<T>,
    id: MutCell<osMessageQueueId_t>,
    cb_mem: StaticMem<CB_SIZE>,
    mq_mem: StaticMem<MQ_SIZE>,
}

impl<T: Copy, const MSG_COUNT: usize, const MQ_SIZE: usize> MessageQueue<T, MSG_COUNT, MQ_SIZE> {
    pub const fn uninit() -> Self {
        Self {
            _p: PhantomData,
            id: MutCell::const_new(null_mut()),
            cb_mem: StaticMem::const_new(),
            mq_mem: StaticMem::const_new(),
        }
    }

    pub unsafe fn init_once(&self, name: &'static str) {
        let attr = osMessageQueueAttr_t {
            name: name.as_ptr() as _,
            attr_bits: 0,
            cb_mem: self.cb_mem.as_ptr(),
            cb_size: CB_SIZE as u32,
            mq_mem: self.mq_mem.as_ptr(),
            mq_size: MQ_SIZE as u32,
        };
        let id = osMessageQueueNew(MSG_COUNT as u32, size_of::<T>() as u32, &attr);
        if id.is_null() {
            panic!("message queue new fail");
        }
        *self.id.as_mut() = id;
    }

    pub fn post(&self, value: &T) -> Result<(), Error> {
        match unsafe { osMessageQueuePut(*self.id, value as *const T as _, 0, 0) } {
            osStatus_t::osOK => Ok(()),
            osStatus_t::osErrorResource => Err(Error::Resource),
            _ => panic!("mq post fail"),
        }
    }

    pub fn poll(&self, timeout: u32) -> Result<T, Error> {
        let mut value: MaybeUninit<T> = MaybeUninit::uninit();
        match unsafe { osMessageQueueGet(*self.id, value.as_mut_ptr() as _, null_mut(), timeout) } {
            osStatus_t::osOK => Ok(unsafe { value.assume_init_read() }),
            osStatus_t::osErrorTimeout => Err(Error::Timeout),
            _ => panic!("mq poll fail"),
        }
    }

    pub fn take(&self) -> T {
        let mut value: MaybeUninit<T> = MaybeUninit::uninit();
        unsafe {
            if osMessageQueueGet(*self.id, value.as_mut_ptr() as _, null_mut(), osWaitForever)
                != osStatus_t::osOK
            {
                panic!("mq take fail");
            }
            value.assume_init_read()
        }
    }

    pub fn reset(&self) {
        unsafe {
            if osStatus_t::osOK != osMessageQueueReset(*self.id) {
                panic!("mq reset fail");
            }
        }
    }
}
